#-*- coding: UTF-8 -*- 
import cv2
import time
import socket
import nanocamera as nano
from utils import im2coordinate
import os
import numpy as np
import glob
import pickle
from map2array_lib import map2array , array2senddata,generate_start_end
from uart_example import get_uart_data
import serial
# 服务端ip地址
HOST = '192.168.1.112'
# 服务端端口号
PORT = 8080
ADDRESS = (HOST, PORT)
width=1280
height=720
camera = nano.Camera(device_id=0, flip=0, width=width, height=height, fps=15)  #
#camera = nano.Camera(flip=2, width=width, height=height, fps=15)

serial_port = serial.Serial(
    port="/dev/ttyTHS1",
    baudrate=115200,
    bytesize=serial.EIGHTBITS,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
)

# 创建一个套接字
tcpClient = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# 连接远程ip
tcpClient.connect(ADDRESS)

with open('mtx.pickle',"rb") as f1:
    mtx = pickle.load(f1)
with open('dist.pickle',"rb") as f1:
    dist = pickle.load(f1)

w=width
h=height
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
h,  w = height,width 
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w,h), 5)
def ten2byteofhex(num):
    hex_str=str(hex(num))[2:]
    if len(hex_str)==1:
        hex_str='0'+hex_str
    return(bytes.fromhex(hex_str))

while True:
    num=len(os.listdir("./shot"))
    # 计时
    start = time.perf_counter()
    # 读取图像
    cv_image = camera.read()
    
    dst = cv2.remap(cv_image, mapx, mapy, cv2.INTER_LINEAR)
    # crop the image
    x, y, w, h = roi
    cv_image= dst[y:y+h, x:x+w]

    if len(os.listdir("./shot/shot_flag"))==1:
        cv2.imwrite("./shot/"+str(num-1)+".png",cv_image)
        os.remove("/home/xd233/shot/shot_flag/1")
        print(str(num-1)+".png is saved")
    #x_record=im2coordinate(cv_image)
   # cv2.circle(cv_image,(x_record,615),5,(213,0,0),-1)
    uart_data=get_uart_data()#jetson nano 接收到的数据，根据\n来分段
    if len(uart_data)>1: 
        print(uart_data)
    if uart_data=="capture\r\n":
        uart_data=""
        data_array,crop_map=map2array(cv_image)#迷宫对应的数组数据
        if data_array is not None:
            start_p=[0,0]
            end_p=[6,0]
            send_data=array2senddata(data_array,start_p,end_p)#生成要发送给stm32的数据
            data_num=len(send_data)
            serial_port.write(bytes.fromhex("AA02")+ten2byteofhex(data_num)+ten2byteofhex(0))
            for i in range(data_num):
                serial_port.write(ten2byteofhex(send_data[i][0]))
                serial_port.write(ten2byteofhex(send_data[i][1]))
            serial_port.write(bytes.fromhex("FF"))
            print(send_data)
        else:
            print("capture is error")
    cv_image=cv2.resize(cv_image,(int(w//2),int(h//2)))
    # 压缩图像
    img_encode = cv2.imencode('.jpg', cv_image, [cv2.IMWRITE_JPEG_QUALITY, 99])[1]
    # 转换为字节流
    bytedata = img_encode.tostring()
    # 标志数据，包括待发送的字节流长度等数据，用‘,’隔开
    flag_data = (str(len(bytedata))).encode() + ",".encode() + " ".encode()

    tcpClient.send(flag_data)
    # 接收服务端的应答
    data = tcpClient.recv(1024)
    if ("ok" == data.decode()):
        # 服务端已经收到标志数据，开始发送图像字节流数据
        tcpClient.send(bytedata)
    # 接收服务端的应答
    data = tcpClient.recv(1024)
    if ("ok" == data.decode()):
        # 计算发送完成的延时
       # print("延时：" + str(int((time.perf_counter() - start) * 1000)) + "ms")
       pass
